/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/publicInterfaceSrc/pid.cpp
 * @Description  : PID控制器类，用于根据输入误差和当前速度生成控制量。
 * 				   其中包括构造函数、清除控制参数的方法、选择PID滤波器的方法，以及生成控制量的方法。
 *                 控制器通过比例、积分和微分参数来计算控制量，并包含积分限和速度判断来决定是否添加积分项。
 *                 如果检测到NaN错误或速度过低，则会清除控制器的输出。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:18:48
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <math.h> 
#include "pid.h"
#include "tools.h" 
//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CPid".
//
//   The constructor.
/**
 * @brief PidController 类的构造函数
 *
 * 构造函数初始化 PidController 对象，并调用 ClearControl 方法清除控制参数。
 */
PidController::PidController() 
{
  	ClearControl();
}

/**
 * @brief 清空控制器输出和误差记录
 *
 * 将控制器的输出、上一次误差和上上一次的误差都清零。
 */
void PidController::ClearControl() 
{
	control_output_ = 0.0f;
	previous_error1_ = 0.0f;
	previous_error2_ = 0.0f;
}

/**
 * @brief 选择PID控制器的滤波参数
 *
 * 根据传入的参数选择使用低速模式或高速模式的PID滤波器。
 *
 * @param low_speed 是否使用低速模式，true表示使用低速模式，false表示使用高速模式
 * @param filter PID滤波器对象，用于PID控制器的滤波参数
 */
void PidController::SelectPid(bool low_speed, const PidFilter& filter) 
{
	current_filter_ = filter;
	DEBUG_OUT("SelectPid: use low_speed = " << low_speed);
}

/**
 * @brief 根据输入误差和当前速度生成PID控制量
 *
 * 根据输入误差和当前速度，通过PID算法计算出控制量。
 *
 * @param input_error 输入误差
 * @param real_vel 当前速度
 *
 * @return 返回控制量
 */
float PidController::Generate(float input_error, float real_vel) 
{
	// 获取PID控制器的参数
	float kp = current_filter_.kp;
	float ki = current_filter_.ki;
	float kd = current_filter_.kd;
	float il = current_filter_.il;
	// 计算当前控制量
	float current_control = (kp + kd) * input_error - (2 * kd + kp) * previous_error1_ + kd * previous_error2_;
    
	// 如果输入误差在"积分限"内，且实际速度大于0.0001，则添加"积分项"
	// If the input is within the "Integral Limit", add the "Integral Item"
	if (fabs(input_error) < il && fabsf(real_vel) > 0.0001f) {
		current_control += ki * input_error;
	}
    // 更新前一个误差
	previous_error2_ = previous_error1_;
	previous_error1_ = input_error;

	// 输出控制量
	control_output_ += current_control;

	if (fabsf(real_vel) <= IGNORE_CORRECT_VEL) {
		ClearControl();
		DEBUG_OUT("ignore pid out; current vel=" << real_vel);
	}
	// 如果前一个误差或控制输出为NaN，则强制忽略PID输出并清除控制
	if (std::isnan(previous_error1_) || std::isnan(previous_error2_) || std::isnan(control_output_)) {
		DEBUG_ERROR_OUT("force ignore pid out; current vel=" << real_vel
							<< ", previous_error1_=" << previous_error1_
							<< ", previous_error2_=" << previous_error2_
							<< ", control_output_=" << control_output_);
		ClearControl();
	}

	return control_output_;
}
